load("//tools/lint:lint.bzl", "add_lint_tests")
load(
    "//tools/skylark:drake_cc.bzl",
    "drake_cc_googletest",
    "drake_cc_library",
    "drake_cc_package_library",
)

package(
    default_visibility = ["//visibility:public"],
)

drake_cc_package_library(
    name = "sap",
    visibility = ["//visibility:public"],
    deps = [
        ":contact_problem_graph",
        ":dense_supernodal_solver",
        ":partial_permutation",
        ":sap_ball_constraint",
        ":sap_constraint",
        ":sap_constraint_bundle",
        ":sap_constraint_jacobian",
        ":sap_contact_problem",
        ":sap_coupler_constraint",
        ":sap_distance_constraint",
        ":sap_fixed_constraint",
        ":sap_friction_cone_constraint",
        ":sap_holonomic_constraint",
        ":sap_hunt_crossley_constraint",
        ":sap_limit_constraint",
        ":sap_model",
        ":sap_pd_controller_constraint",
        ":sap_solver",
        ":sap_solver_results",
        ":sap_tendon_constraint",
        ":sap_weld_constraint",
    ],
)

drake_cc_library(
    name = "contact_problem_graph",
    srcs = ["contact_problem_graph.cc"],
    hdrs = ["contact_problem_graph.h"],
    deps = [
        ":partial_permutation",
        "//common:essential",
        "//common:sorted_pair",
    ],
)

drake_cc_library(
    name = "dense_supernodal_solver",
    srcs = ["dense_supernodal_solver.cc"],
    hdrs = ["dense_supernodal_solver.h"],
    deps = [
        "//common:essential",
        "//multibody/contact_solvers:supernodal_solver",
    ],
)

drake_cc_library(
    name = "expect_equal",
    testonly = 1,
    srcs = ["expect_equal.cc"],
    hdrs = ["expect_equal.h"],
    visibility = ["//visibility:private"],
    deps = [
        ":sap_constraint",
        "//common:essential",
        "@gtest//:without_main",
    ],
)

drake_cc_library(
    name = "partial_permutation",
    srcs = ["partial_permutation.cc"],
    hdrs = ["partial_permutation.h"],
    deps = [
        "//common:essential",
    ],
)

drake_cc_library(
    name = "sap_constraint",
    srcs = ["sap_constraint.cc"],
    hdrs = ["sap_constraint.h"],
    deps = [
        ":sap_constraint_jacobian",
        "//common:default_scalars",
        "//common:essential",
        "//common:unused",
        "//common:value",
        "//multibody/contact_solvers:matrix_block",
        "//multibody/contact_solvers/sap:partial_permutation",
        "//multibody/math:spatial_algebra",
        "//multibody/plant:slicing_and_indexing",
    ],
)

drake_cc_library(
    name = "sap_constraint_bundle",
    srcs = ["sap_constraint_bundle.cc"],
    hdrs = ["sap_constraint_bundle.h"],
    deps = [
        ":partial_permutation",
        ":sap_contact_problem",
        "//common:default_scalars",
        "//common:essential",
        "//multibody/contact_solvers:block_sparse_matrix",
    ],
)

drake_cc_library(
    name = "sap_constraint_jacobian",
    srcs = ["sap_constraint_jacobian.cc"],
    hdrs = ["sap_constraint_jacobian.h"],
    deps = [
        "//common:default_scalars",
        "//common:essential",
        "//math:autodiff",
        "//multibody/contact_solvers:matrix_block",
    ],
)

drake_cc_library(
    name = "sap_contact_problem",
    srcs = ["sap_contact_problem.cc"],
    hdrs = ["sap_contact_problem.h"],
    deps = [
        ":contact_problem_graph",
        ":sap_constraint",
        ":sap_solver_results",
        "//common:default_scalars",
        "//common:essential",
        "//multibody/contact_solvers:block_sparse_matrix",
        "//multibody/plant:slicing_and_indexing",
    ],
)

drake_cc_library(
    name = "sap_ball_constraint",
    srcs = ["sap_ball_constraint.cc"],
    hdrs = ["sap_ball_constraint.h"],
    deps = [
        ":sap_constraint",
        ":sap_constraint_jacobian",
        ":sap_holonomic_constraint",
        "//common:default_scalars",
        "//common:essential",
    ],
)

drake_cc_library(
    name = "sap_coupler_constraint",
    srcs = ["sap_coupler_constraint.cc"],
    hdrs = ["sap_coupler_constraint.h"],
    deps = [
        ":sap_constraint",
        ":sap_constraint_jacobian",
        ":sap_holonomic_constraint",
        "//common:default_scalars",
        "//common:essential",
    ],
)

drake_cc_library(
    name = "sap_distance_constraint",
    srcs = ["sap_distance_constraint.cc"],
    hdrs = ["sap_distance_constraint.h"],
    deps = [
        ":sap_constraint",
        ":sap_constraint_jacobian",
        ":sap_holonomic_constraint",
        "//common:default_scalars",
        "//common:essential",
    ],
)

drake_cc_library(
    name = "sap_fixed_constraint",
    srcs = ["sap_fixed_constraint.cc"],
    hdrs = ["sap_fixed_constraint.h"],
    deps = [
        ":sap_constraint",
        ":sap_constraint_jacobian",
        ":sap_holonomic_constraint",
        "//common:default_scalars",
        "//common:essential",
    ],
)

drake_cc_library(
    name = "sap_tendon_constraint",
    srcs = ["sap_tendon_constraint.cc"],
    hdrs = ["sap_tendon_constraint.h"],
    deps = [
        ":sap_constraint",
        ":sap_constraint_jacobian",
        "//common:default_scalars",
        "//common:essential",
    ],
)

drake_cc_library(
    name = "sap_holonomic_constraint",
    srcs = ["sap_holonomic_constraint.cc"],
    hdrs = ["sap_holonomic_constraint.h"],
    deps = [
        ":sap_constraint",
        ":sap_constraint_jacobian",
        "//common:default_scalars",
        "//common:essential",
        "//math:autodiff",
    ],
)

drake_cc_library(
    name = "sap_hunt_crossley_constraint",
    srcs = ["sap_hunt_crossley_constraint.cc"],
    hdrs = ["sap_hunt_crossley_constraint.h"],
    deps = [
        ":sap_constraint",
        ":sap_constraint_jacobian",
        "//common:default_scalars",
        "//common:essential",
        "//multibody/contact_solvers:contact_configuration",
    ],
)

drake_cc_library(
    name = "sap_limit_constraint",
    srcs = ["sap_limit_constraint.cc"],
    hdrs = ["sap_limit_constraint.h"],
    deps = [
        ":sap_constraint",
        ":sap_constraint_jacobian",
        "//common:default_scalars",
        "//common:essential",
    ],
)

drake_cc_library(
    name = "sap_pd_controller_constraint",
    srcs = ["sap_pd_controller_constraint.cc"],
    hdrs = ["sap_pd_controller_constraint.h"],
    deps = [
        ":sap_constraint",
        ":sap_constraint_jacobian",
        "//common:default_scalars",
        "//common:essential",
    ],
)

drake_cc_library(
    name = "sap_friction_cone_constraint",
    srcs = ["sap_friction_cone_constraint.cc"],
    hdrs = ["sap_friction_cone_constraint.h"],
    deps = [
        ":sap_constraint",
        ":sap_constraint_jacobian",
        "//common:default_scalars",
        "//common:essential",
        "//multibody/contact_solvers:contact_configuration",
    ],
)

drake_cc_library(
    name = "sap_weld_constraint",
    srcs = ["sap_weld_constraint.cc"],
    hdrs = ["sap_weld_constraint.h"],
    deps = [
        ":sap_constraint",
        ":sap_constraint_jacobian",
        ":sap_holonomic_constraint",
        "//common:default_scalars",
        "//common:essential",
    ],
)

drake_cc_library(
    name = "sap_model",
    srcs = ["sap_model.cc"],
    hdrs = ["sap_model.h"],
    deps = [
        ":contact_problem_graph",
        ":dense_supernodal_solver",
        ":partial_permutation",
        ":sap_constraint_bundle",
        ":sap_contact_problem",
        "//common:default_scalars",
        "//common:essential",
        "//math:linear_solve",
        "//multibody/contact_solvers:block_sparse_matrix",
        "//multibody/contact_solvers:block_sparse_supernodal_solver",
        "//systems/framework:context",
        "//systems/framework:leaf_system",
    ],
)

drake_cc_library(
    name = "sap_solver",
    srcs = ["sap_solver.cc"],
    hdrs = ["sap_solver.h"],
    deps = [
        ":sap_model",
        ":sap_solver_results",
        "//common:default_scalars",
        "//common:essential",
        "//math:linear_solve",
        "//multibody/contact_solvers:block_sparse_matrix",
        "//multibody/contact_solvers:block_sparse_supernodal_solver",
        "//multibody/contact_solvers:newton_with_bisection",
        "//multibody/contact_solvers:point_contact_data",
        "//multibody/contact_solvers:supernodal_solver",
        "//multibody/contact_solvers:system_dynamics_data",
    ],
)

drake_cc_library(
    name = "sap_solver_results",
    srcs = ["sap_solver_results.cc"],
    hdrs = ["sap_solver_results.h"],
    deps = ["//common:default_scalars"],
)

drake_cc_googletest(
    name = "dense_supernodal_solver_test",
    deps = [
        ":dense_supernodal_solver",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
    ],
)

drake_cc_library(
    name = "validate_constraint_gradients",
    testonly = 1,
    srcs = ["validate_constraint_gradients.cc"],
    hdrs = ["validate_constraint_gradients.h"],
    visibility = ["//visibility:private"],
    deps = [
        ":sap_constraint",
        "//common/test_utilities:eigen_matrix_compare",
        "//math:gradient",
    ],
)

drake_cc_googletest(
    name = "partial_permutation_test",
    deps = [
        ":partial_permutation",
        "//common/test_utilities:expect_throws_message",
    ],
)

drake_cc_googletest(
    name = "sap_constraint_test",
    deps = [
        ":expect_equal",
        ":sap_constraint",
    ],
)

drake_cc_googletest(
    name = "sap_constraint_bundle_test",
    deps = [
        ":partial_permutation",
        ":sap_constraint_bundle",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
        "//math:gradient",
        "//multibody/contact_solvers:block_sparse_matrix",
    ],
)

drake_cc_googletest(
    name = "sap_contact_problem_test",
    deps = [
        ":sap_constraint",
        ":sap_contact_problem",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
    ],
)

drake_cc_googletest(
    name = "sap_ball_constraint_test",
    deps = [
        ":expect_equal",
        ":sap_ball_constraint",
        ":validate_constraint_gradients",
        "//common:pointer_cast",
        "//common/test_utilities:eigen_matrix_compare",
    ],
)

drake_cc_googletest(
    name = "sap_coupler_constraint_test",
    deps = [
        ":expect_equal",
        ":sap_coupler_constraint",
        ":validate_constraint_gradients",
        "//common:pointer_cast",
        "//common/test_utilities:eigen_matrix_compare",
    ],
)

drake_cc_googletest(
    name = "sap_distance_constraint_test",
    deps = [
        ":expect_equal",
        ":sap_distance_constraint",
        ":validate_constraint_gradients",
        "//common:pointer_cast",
        "//common/test_utilities:eigen_matrix_compare",
    ],
)

drake_cc_googletest(
    name = "sap_fixed_constraint_test",
    deps = [
        ":expect_equal",
        ":sap_fixed_constraint",
        ":validate_constraint_gradients",
        "//common:pointer_cast",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
    ],
)

drake_cc_googletest(
    name = "sap_tendon_constraint_test",
    deps = [
        ":expect_equal",
        ":sap_tendon_constraint",
        ":validate_constraint_gradients",
        "//common:pointer_cast",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
    ],
)

drake_cc_googletest(
    name = "sap_holonomic_constraint_test",
    deps = [
        ":expect_equal",
        ":sap_holonomic_constraint",
        ":validate_constraint_gradients",
        "//common:pointer_cast",
        "//common/test_utilities:eigen_matrix_compare",
    ],
)

drake_cc_googletest(
    name = "sap_hunt_crossley_constraint_test",
    deps = [
        ":expect_equal",
        ":sap_hunt_crossley_constraint",
        ":validate_constraint_gradients",
        "//common:pointer_cast",
        "//common/test_utilities:eigen_matrix_compare",
        "//math:gradient",
        "//solvers:constraint",
    ],
)

drake_cc_googletest(
    name = "sap_limit_constraint_test",
    deps = [
        ":expect_equal",
        ":sap_limit_constraint",
        ":validate_constraint_gradients",
        "//common:pointer_cast",
        "//common/test_utilities:eigen_matrix_compare",
    ],
)

drake_cc_googletest(
    name = "sap_pd_controller_constraint_test",
    deps = [
        ":expect_equal",
        ":sap_pd_controller_constraint",
        ":validate_constraint_gradients",
        "//common:pointer_cast",
        "//common/test_utilities:eigen_matrix_compare",
    ],
)

drake_cc_googletest(
    name = "sap_friction_cone_constraint_test",
    deps = [
        ":expect_equal",
        ":sap_friction_cone_constraint",
        ":validate_constraint_gradients",
        "//common:pointer_cast",
        "//common/test_utilities:eigen_matrix_compare",
        "//math:gradient",
        "//solvers:constraint",
        "//solvers:mathematical_program",
        "//solvers:scs_solver",
        "//solvers:solve",
    ],
)

drake_cc_googletest(
    name = "sap_weld_constraint_test",
    deps = [
        ":expect_equal",
        ":sap_weld_constraint",
        ":validate_constraint_gradients",
        "//common:pointer_cast",
        "//common/test_utilities:eigen_matrix_compare",
    ],
)

drake_cc_googletest(
    name = "contact_problem_graph_test",
    deps = [
        ":contact_problem_graph",
    ],
)

drake_cc_googletest(
    name = "sap_model_test",
    deps = [
        ":sap_constraint",
        ":sap_contact_problem",
        ":sap_model",
        "//common/test_utilities:eigen_matrix_compare",
        "//math:gradient",
    ],
)

drake_cc_googletest(
    name = "sap_solver_autodiff_test",
    data = [
        "@drake_models//:dishes",
        "@drake_models//:iiwa_description",
    ],
    deps = [
        ":sap_solver",
        ":sap_solver_results",
        "//common:find_resource",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:maybe_pause_for_user",
        "//multibody/parsing",
        "//multibody/plant",
        "//multibody/plant:compliant_contact_manager_tester",
        "//multibody/test_utilities:robot_model",
        "//visualization:visualization_config_functions",
    ],
)

drake_cc_googletest(
    name = "sap_solver_test",
    deps = [
        ":sap_friction_cone_constraint",
        ":sap_solver",
        ":sap_solver_results",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
        "//multibody/contact_solvers:block_sparse_matrix",
        "//multibody/contact_solvers:contact_solver_utils",
    ],
)

add_lint_tests()
